5. 复杂任务与系统联调¶

5.1 独立模块实现（脚本篇）¶

为什么要用脚本（独立化脚本）？

   脚本（如 Python）适合需高灵活、自动化、跨平台联动的模块，能突破 Omnigraph 的功能边界，核心价值有三：

  1. 复杂逻辑可定制：Omnigraph 预制节点无法覆盖的需求（如 A * 避障、点云分割、多机协同），脚本可通过代码编写算法。比如用 Python 实现 AGV “动态躲避临时障碍”，调用 OpenCV 处理图像、计算路径，实现定制化控制。

  2. 批量自动化提效：支持循环、条件判断，能批量生成场景、自动跑仿真、导出数据。比如机械臂精度测试，脚本可一键生成 100 组关节指令、自动记录误差并导出表格，10 分钟完成原本几小时的手动操作。

  3. 跨平台联动衔接：可对接 Isaac Sim 外系统（ROS 2、MATLAB、数据分析库），比如用 Python 脚本将仿真数据发至 ROS 2，或导出到 Pandas 做分析，是仿真模块与实际工程落地的关键桥梁。

5.1.1 导航仿真案例¶

注意：本章节导航建图部分可以参考4.2.1

5.1.1.1 导航设计思路¶

   核心知识点：导航案例涉及 “SLAM 建图 + 路径规划”。

   导航的核心是传感器：激光雷达、里程计。由于X1机器人本体的激光雷达在仿真平台的ActionGraph中不适配（当然也可以用类似的传感器在仿真中替代原来的激光雷达，理论上只需要传感器数据一致即可），因此通过独立化脚本的方式，实现激光雷达的采集数据、发布数据等步骤。

   所以导航的整体思路是：在仿真中模拟真机X1机器人采集数据，并通过ROS2话题将数据发布给导航功能包Nav2，导航功能包接收数据后输出“Control instruction”，最终仿真机器人底盘接收，从而实现驱动机器人底盘运动（如下图）

exported_image

5.1.1.2 导航模块Demo执行¶

该导航Demo具备在仿真平台上，通过rviz可视化界面，完成点到点的导航任务。而独立化脚本则是替代了原有的仿真平台启动方式以及ActionGraph的实现，用python代码的形式实现ROS订阅与发布，从而实现控制仿真机器人。

一、进入isaac-sim容器

  1. 执行命令：docker exec -it isaac-lab-course-test bash

  2. 执行独立化脚本: python run_custom_navigation.py

  3. 出现以下日志时，证明启动成功：

exported_image

  1. 打开isaacsim-webrtc-streaming-client，输入IP后进入仿真：

exported_image

  1. 可以看到运行效果：（演示导航用的机器人模型暂不涉及机械臂）

exported_image

  到这里已经通过独立化脚本，将仿真模块成功启动，接下去ROS2容器中启动导航功能包即可。

二、进入ROS2容器

  1. 完成2.2.1.2的建图与配置后

  2. 编译功能包：colcon build --packages-select carter_navigation

  3. 添加以下命令至~/.bashrc中，以免每次执行都要配置环境。

source /root/humble_ws/install/local_setup.bash
export FASTRTPS_DEFAULT_PROFILES_FILE=/root/humble_ws/fastdds.xml
export DISPLAY=:1

  1. 完成添加后，执行一次：source ~/.bashrc。（仅需配置一次，之后每次执行无需理会环境）

  2. 执行以下启动命令：ros2 launch carter_navigation carter_navigation.launch.py use_sim_time:=true

  3. 出现以下日志、服务器宿主机出现rviz并显示机器人和场景时，证明启动成功：

exported_image

exported_image

  1. 若出现以下相关报错，执行该命令：xhost +就能解决：

exported_image

   执行效果：

exported_image

   通过启动仿真的独立化脚本、ROS2的导航功能包，在rviz2界面中，设置目标点（Nav2_Goal）后，机器人在仿真中实现从A点到B点的导航。

exported_image

5.1.2 视觉仿真案例¶

注意：本章节关于视觉识别的原理与Apriltag的相关知识可以参考4.2.2

5.1.2.1 实现思路与方法¶

   通过将硬件设备投入仿真中，获取深度相机、RGB相机的数据，经过检测与处理，得到目标物体的坐标与位姿，最终将其发给机械臂进行抓取。

   本项目的视觉系统采用与双臂路径规划相同的架构，即 ROS2 容器与 Isaac Sim 仿真容器相结合的架构。其中，ROS2 容器负责视觉算法的处理和计算，Isaac Sim 仿真容器负责机器人的控制和仿真。

   数据流程设计：

  • 图像获取：Isaac Sim 仿真容器中的机器人头部双目相机拍摄包含二维码的图像。
  • 图像传输：将拍摄的图像通过 ROS 话题传输到 ROS2 容器中。
  • 图像处理与识别：ROS2 容器中的视觉算法对图像进行处理，识别二维码并计算其中心像素坐标。
  • 深度获取：通过双目相机计算出的深度信息，计算二维码在相机坐标系中的位置。
  • 坐标转换：将二维码在相机坐标系中的位置转换到世界坐标系中。
  • 位置传输：将二维码在世界坐标系中的位置通过 ROS 话题传输给机械臂模块。
  • 路径规划：Isaac Sim 仿真容器中的机械臂模块会根据二维码的位置进行路径规划，实现对二维码的抓取。

exported_image

5.1.2.2 相机模块运行Demo¶

相机模块双目相机Demo，旨在让用户将流程跑通，理解相机模块的关键核心点。

   我把demo的运行步骤给大家阐述一遍，即便是新手也能轻松入门！

  1. 在主机终端上输入，进入仿真容器：docker exec -it isaacsim bash

  2. 进入项目目录：cd /isaac-sim/x1-manipulator

   目录结构：

x1-manipulator/
├── USD_Save/
├── april_detect/  # 相机与AprilTag检测功能模块
│   ├── Binocular-vision/  # 双目相机检测子模块
│   └── depth-cam/  # 深度相机检测子模块
└── mercury_dual/

该demo任务是X1的双目相机通过相机识别算法，在仿真中动态获取目标二维码april_tag表面中心坐标。

   进入相机目录：cd /x1-manipulator/april_detect/Binocular-vision

   执行demo.py：python demo_Bino_course.py

   运行效果如下：

   日志：

exported_image

   效果图：（这里的双目相机案例用的是虚拟相机，所以在仿真页面上看不见）

exported_image

   拖动二维码方块的效果图：

exported_image

   整体效果动图如下：（沿着xyz三轴坐标移动，三轴坐标值随之增大）

exported_image

注意：这里只展示了双目相机demo，代码中还有深度相机，感兴趣的可以用同样的方式与步骤启动尝试一下。

5.1.3 双臂仿真案例¶

5.1.3.1 案例目标与实现思路¶

5.1.3.1.1 双臂协同抓取¶

   本项目旨在实现双臂机器人的双臂协同抓取任务。即让双臂机器人的两个手臂能够相互配合，在复杂的场景中准确抓取目标物体。通过精确的路径规划和协调控制，使双臂在抓取过程中避免碰撞，高效、稳定地完成抓取任务，为实际应用如物流搬运、工业装配等提供技术支持。

   下图是来自于官方的单臂机器人抓取任务示意图。

exported_image

5.1.3.1.2 思路与架构图¶

   首先，在 Isaccsim 仿真容器中搭建好包含机器人和场景的 usd 文件，构建逼真的仿真环境。接着，在 ros2 容器中完成 curobo 环境的装配与配置，确保 curobo 能够正常运行。然后，基于目标物体的位置和姿态，利用 curobo 计算双臂机器人每个关节的运动角度，规划出双臂的运动路径。在路径规划过程中，要考虑双臂之间的协同关系，避免运动干涉。最后，将规划好的路径发送给机器人，在仿真环境中进行测试和验证，根据测试结果对路径规划算法进行优化和调整，直至实现稳定、高效的双臂协同抓取任务。

exported_image

5.1.3.2 cuRobo介绍¶

   cuRobo，全称为CUDA加速机器人库，是一个由NVIDIA开发的开源项目，专注于为机器人提供高效、准确的逆解计算。cuRobo集成了多种先进的机器人算法，包括正逆运动学、碰撞检测、数值优化、几何规划、轨迹优化以及运动生成等。在双臂机器人路径规划中，它能快速根据机器人末端执行器的目标位置和姿态，计算出每个关节的角度，从而实现机器人手臂的灵活运动。通过使用curobo，开发者可以避免复杂的运动学计算，将更多精力投入到机器人任务逻辑的开发中，极大提高了开发效率。

exported_image

   cuRobo的核心优势在于其对CUDA技术的深度应用，这使得原本需要大量计算资源的机器人算法能够在GPU上并行处理，从而大幅缩短计算时间。具体来说，cuRobo实现了以下几个关键技术：

  • 正逆运动学：通过CUDA加速，cuRobo能够在极短的时间内完成复杂的运动学计算，这对于实时机器人控制至关重要。
  • 碰撞检测：cuRobo支持多种世界表示形式（如立方体、网格和深度图像），并能在GPU上快速进行碰撞检测，确保机器人操作的安全性。
  • 数值优化：cuRobo集成了多种优化算法，包括梯度下降、L-BFGS和MPPI，这些算法在CUDA的加速下能够更快地找到最优解。
  • 几何规划与轨迹优化：cuRobo通过并行处理多个种子，能够在30毫秒内生成全局运动轨迹，同时优化轨迹的平滑度和长度。

5.1.3.4 cuRobo 逆运动学核心算法解析¶

   cuRobo 的逆运动学求解采用数值优化 + 解析解的混合策略，核心流程如下：

  1. 解析解初始化：利用 X1 机械臂的 DH 参数构建封闭解表达式，快速生成初始关节角度
  2. GPU 并行优化：通过 CUDA 加速 L-BFGS 算法，迭代优化关节角度使末端误差最小化
  3. 约束处理：
  • 关节角度限制（如 1.2.1.3 节所述范围）
  • 避障约束（基于 Octomap 障碍物地图）
  • 双臂协同约束（避免左右臂碰撞）
5.1.3.4.1 与正运动学（FK）的对比¶

   为了更好理解 IK，我们先对比其 “对立面”—— 正运动学（Forward Kinematics, FK）：

  • 正运动学（FK）：已知每个关节的运动参数（如旋转角度、位移距离），计算末端执行器的最终位置和姿态。
    • 例如：已知机器人 “肩膀转 30°、肘部转 60°”，计算 “手部最终在哪里”。
    • 特点：计算简单直接（通过关节链的几何关系推导），但当需要精确控制末端位置时，需要手动调整每个关节参数，效率极低。
  • 逆运动学（IK）：已知末端执行器的目标位置和姿态，反推各个关节应有的运动参数。
    • 例如：已知 “手部要抓到桌子上的杯子”，计算 “肩膀和肘部需要转多少度”。
    • 特点：无需手动调整每个关节，直接通过目标位置反推关节运动，更符合人类直觉，适用于需要精确控制末端位置的场景。
5.1.3.4.2 IK求解器¶

   IK 求解器是实现逆运动学计算的核心工具，其核心目标是在满足关节约束的前提下，让末端执行器精准匹配目标位置，并保证运动自然。根据解算逻辑可分为两类：

  1. 解析法求解器（Analytical Solvers）
  • 原理：通过几何或代数公式推导关节参数的解析解（即数学上的精确解）。 例如，2D 两关节手臂（肩关节 O1、肘关节 O2，连杆长度 L1、L2，末端目标点 P (x,y)）： 可通过余弦定理推导肘关节角度 θ2 = arccos [(x²+y²-L1²-L2²)/(2L1L2)]，再通过反正切函数计算肩关节角度 θ1，直接得到精确解。
  • 适用场景：结构简单的关节链（如 2D 两关节、3D 三关节机械臂），关节数量少（通常≤3 个）且运动关系可通过闭合几何公式表达。
  • 优点：计算速度极快（无迭代过程，直接代入公式）、精度理论上无误差。
  • 缺点：仅适用于特定结构，复杂关节链（如多关节机器人手臂）无法推导解析解，灵活性差。
  1. 数值法求解器（Numerical Solvers）

   当关节链结构复杂（如 7 自由度机械臂、动画角色的四肢），无法通过公式直接求解时，需采用数值法迭代逼近解。常见类型包括：

  • Jacobian 转置法
    • 原理：通过 Jacobian 矩阵（描述关节速度与末端速度关系的矩阵），将末端位置误差 “映射” 为关节调整量，逐步迭代减小误差。
    • 过程：先计算当前末端与目标的误差，再用 Jacobian 矩阵的转置将误差转换为关节旋转角度的调整值，重复迭代直到误差小于阈值。
    • 优点：计算量小，适合实时场景。
    • 缺点：可能因迭代发散导致关节运动异常（如角度突然增大），精度较低。
  • 阻尼最小二乘法（DLS）
    • 原理：在 Jacobian 转置法基础上加入 “阻尼项”（小常数），避免矩阵病态（如 Jacobian 接近奇异时的数值不稳定）。
    • 优势：解决了 Jacobian 转置法的发散问题，能在冗余自由度系统中找到稳定解（如 7 自由度手臂的肘部自然摆放）。
    • 适用场景：工业机器人路径规划、高精度末端控制。
  • 循环坐标下降法（CCD）
    • 原理：从末端关节向根关节 “逐节调整”，每次只优化一个关节，使末端更接近目标。
    • 例如，调整 3D 手臂时，先动腕关节，再动肘关节，最后动肩关节，循环迭代直到末端达标。
    • 优点：实现简单、对关节约束（如旋转范围）支持友好，运动更自然。
    • 适用场景：计算机动画（角色肢体运动）、游戏角色控制。
5.1.3.4.3 cuRobo核心工作原理¶

   cuRobo 是一款专注于机器人运动规划的高性能库，尤其擅长处理双臂机器人等复杂系统的 IK 解算与路径规划。其核心工作原理可从 “架构设计 - 数据载体 - 解算流程” 三方面展开：

  1. 架构选择：ROS2 与 Isaac Sim 的协同优势：

  • 架构选择：采用 ROS2 容器与 Isaccsim 仿真容器相结合的架构。

    • ROS2 作为机器人的 “大脑中枢”，负责处理传感器数据、执行路径规划算法（如 cuRobo 的 IK 解算）、发布关节控制指令。ROS2 的分布式通信能力（如话题、服务机制），可高效连接机器人的感知、规划、控制模块。
    • Isaccsim 仿真容器则为机器人和场景提供了逼真的仿真环境，便于在虚拟环境中测试和验证机器人的行为。

  • 文件执行：在 Isaccsim 仿真容器中打开并执行 usd 文件，usd 文件包含了机器人模型和场景信息，为机器人的运动提供虚拟载体。

  • 环境装配：在 ros2 容器中装配 curobo 环境，借助 curobo 强大的逆解能力，实现双臂机器人的路径规划，让机器人手臂能够按照规划好的路径运动。

  1. 数据载体：USD 文件的核心作用：

   USD（Universal Scene Description，通用场景描述）文件是连接虚拟环境与机器人模型的 “数字蓝图”，包含三类关键信息：

  • 机器人模型数据：关节链结构（如肩关节 - 肘关节 - 腕关节的连接关系）、连杆尺寸、关节约束（如肘关节旋转范围 - 90°~120°）、运动学参数（如连杆质量、惯性矩阵）。
  • 场景信息：虚拟环境中的物体（如桌子、工具、待抓取零件）的位置、尺寸、物理属性（如材质硬度、是否可碰撞）。
  • 动画与交互逻辑：预设的机器人动作序列（如抓取姿态）、场景触发条件（如零件被触碰时的反馈）。

5.1.3.3 资源导入与环境配置¶

5.1.3.3.1 cuRobo安装教程¶
  1. 首先确定已成功安装好curobo库，安装方式：
  • 使用我们制作好的带有curobo库的镜像
  • 进入官网按照教程下载。https://curobo.org/get_started/1_install_instructions.html#library-installation

我们用Python独立库的方式安装的，而非“Install for use in Isaac Sim”

  1. 进入\mercury_dual\IsaacSim-ros_workspaces\humble_ws目录下，使用colcon build编译整个工作空间

  2. 编译完成后，进行环境的配置。

说明：¶

   当前文件里面的函数只是调用curobo库的相关api进行规划，规划的具体实现，还请查阅curobo的相关文档。

   网址：https://curobo.org/get_started/7_api.html

5.1.3.3.2 运行环境配置¶

   进入ROS2容器: docker exec -it ros2 /bin/bash

   运行前的环境配置：

cd /root/x1-manipulator/mercury_dual/IsaacSim-ros_workspaces/humble_ws
rm -r build log install
colcon build 
source install/setup.bash

   环境配置完成后，执行以下操作。

5.1.3.3 双臂模块的执行Demo¶

   进入Isaacsim仿真容器:

docker exec -it isaacsim /bin/bash

   运行仿真场景:

cd /isaac-sim
./runheadless.sh

   Open打开USD文件:/isacc-sim/x1-manipulator/USD_Save/mercury_x1_turing_stack_AddDoubleCamera.usd后，点击Play按钮，启动仿真。

   进入ROS2容器:

cd /root/x1-manipulator/mercury_dual/IsaacSim-ros_workspaces/humble_ws
source install/setup.bash
ros2 run isaac_tutorials curobot.py

   运行curobot.py脚本，控制机械臂运动到目标位置。

   运行效果图：

exported_image

   运行日志：

exported_image